// ##########################################################################
// #                                                                        #
// #                              CLOUDCOMPARE                              #
// #                                                                        #
// #  This program is free software; you can redistribute it and/or modify  #
// #  it under the terms of the GNU General Public License as published by  #
// #  the Free Software Foundation; version 2 or later of the License.      #
// #                                                                        #
// #  This program is distributed in the hope that it will be useful,       #
// #  but WITHOUT ANY WARRANTY; without even the implied warranty of        #
// #  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the          #
// #  GNU General Public License for more details.                          #
// #                                                                        #
// #          COPYRIGHT: EDF R&D / TELECOM ParisTech (ENST-TSI)             #
// #                                                                        #
// ##########################################################################

// Always first
#include "ccOctree.h"

#include "ccIncludeGL.h"

// Local
#include "ccBox.h"
#include "ccCameraSensor.h"
#include "ccNormalVectors.h"
#include "ccPointCloud.h"
#include "ccProgressDialog.h"
#include "ccScalarField.h"

// CCCoreLib
#include <Neighbourhood.h>
#include <RayAndBox.h>
#include <ScalarFieldTools.h>

#ifdef QT_DEBUG
// #define DEBUG_PICKING_MECHANISM
#endif

// System
#include <random>

ccOctree::ccOctree(ccGenericPointCloud* aCloud)
    : CCCoreLib::DgmOctree(aCloud)
    , m_theAssociatedCloudAsGPC(aCloud)
    , m_displayedLevel(1)
    , m_displayMode(WIRE)
    , m_glListID(0)
    , m_glListIsDeprecated(true)
    , m_frustumIntersector(nullptr)
{
}

ccOctree::~ccOctree()
{
	if (m_frustumIntersector)
	{
		delete m_frustumIntersector;
		m_frustumIntersector = nullptr;
	}
}

void ccOctree::setDisplayedLevel(int level)
{
	if (level != m_displayedLevel)
	{
		m_displayedLevel     = level;
		m_glListIsDeprecated = true;
	}
}

void ccOctree::setDisplayMode(DisplayMode mode)
{
	if (m_displayMode != mode)
	{
		m_displayMode        = mode;
		m_glListIsDeprecated = true;
	}
}

void ccOctree::clear()
{
	// warn the others that the octree organization is going to change
	Q_EMIT updated();

	QOpenGLContext* context = QOpenGLContext::currentContext();
	if (context)
	{
		// get the set of OpenGL functions (version 2.1)
		QOpenGLFunctions_2_1* glFunc = context->versionFunctions<QOpenGLFunctions_2_1>();
		assert(glFunc != nullptr);

		if (glFunc && glFunc->glIsList(m_glListID))
		{
			glFunc->glDeleteLists(m_glListID, 1);
		}
	}

	m_glListID           = 0;
	m_glListIsDeprecated = true;

	DgmOctree::clear();
}

ccBBox ccOctree::getSquareBB() const
{
	return ccBBox(m_dimMin, m_dimMax, true);
}

ccBBox ccOctree::getPointsBB() const
{
	return ccBBox(m_pointsMin, m_pointsMax, m_numberOfProjectedPoints != 0);
}

void ccOctree::multiplyBoundingBox(const PointCoordinateType multFactor)
{
	m_dimMin *= multFactor;
	m_dimMax *= multFactor;
	m_pointsMin *= multFactor;
	m_pointsMax *= multFactor;

	for (int i = 0; i <= MAX_OCTREE_LEVEL; ++i)
		m_cellSize[i] *= multFactor;
}

void ccOctree::translateBoundingBox(const CCVector3& T)
{
	m_dimMin += T;
	m_dimMax += T;
	m_pointsMin += T;
	m_pointsMax += T;
}

/*** RENDERING METHODS ***/

void ccOctree::draw(CC_DRAW_CONTEXT& context, ccColor::Rgb* pickingColor /*=nullptr*/)
{
	if (!m_theAssociatedCloudAsGPC
	    || m_thePointsAndTheirCellCodes.empty())
	{
		return;
	}

	// get the set of OpenGL functions (version 2.1)
	QOpenGLFunctions_2_1* glFunc = context.glFunctions<QOpenGLFunctions_2_1>();
	assert(glFunc != nullptr);

	if (glFunc == nullptr)
		return;

	glFunc->glPushAttrib(GL_LIGHTING_BIT);

	if (m_displayMode == WIRE)
	{
		// this display mode is too heavy to be stored as a GL list
		//(therefore we always render it dynamically)

		glFunc->glDisable(GL_LIGHTING);
		if (pickingColor)
			ccGL::Color(glFunc, *pickingColor);
		else
			ccGL::Color(glFunc, ccColor::green);

		void* additionalParameters[] = {reinterpret_cast<void*>(m_frustumIntersector),
		                                reinterpret_cast<void*>(glFunc)};
		executeFunctionForAllCellsAtLevel(m_displayedLevel,
		                                  &DrawCellAsABox,
		                                  additionalParameters);
	}
	else
	{
		glDrawParams glParams;
		m_theAssociatedCloudAsGPC->getDrawingParameters(glParams);

		if (pickingColor)
		{
			ccGL::Color(glFunc, *pickingColor);
			glParams.showSF     = false;
			glParams.showColors = false;
			glParams.showNorms  = false;
		}
		else if (!glParams.showColors)
		{
			ccGL::Color(glFunc, ccColor::white);
		}

		if (glParams.showNorms)
		{
			glFunc->glDisable(GL_RESCALE_NORMAL);
			glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, CC_DEFAULT_CLOUD_AMBIENT_COLOR.rgba);
			glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, CC_DEFAULT_CLOUD_SPECULAR_COLOR.rgba);
			glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, CC_DEFAULT_CLOUD_DIFFUSE_COLOR.rgba);
			glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, CC_DEFAULT_CLOUD_EMISSION_COLOR.rgba);
			glFunc->glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, CC_DEFAULT_CLOUD_SHININESS);
			glFunc->glEnable(GL_LIGHTING);

			glFunc->glEnable(GL_COLOR_MATERIAL);
			glFunc->glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
		}

		// shall we recompile the GL list?
		if (m_glListIsDeprecated)
		{
			if (m_glListID == 0)
			{
				m_glListID = glFunc->glGenLists(1);
			}
			glFunc->glNewList(m_glListID, GL_COMPILE);

			if (m_displayMode == MEAN_POINTS)
			{
				void* additionalParameters[] = {reinterpret_cast<void*>(&glParams),
				                                reinterpret_cast<void*>(m_theAssociatedCloudAsGPC),
				                                reinterpret_cast<void*>(glFunc)};

				if (glParams.showNorms)
				{
					glFunc->glEnable(GL_RESCALE_NORMAL);
					glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, CC_DEFAULT_CLOUD_AMBIENT_COLOR.rgba);
					glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, CC_DEFAULT_CLOUD_SPECULAR_COLOR.rgba);
					glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, CC_DEFAULT_CLOUD_DIFFUSE_COLOR.rgba);
					glFunc->glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, CC_DEFAULT_CLOUD_EMISSION_COLOR.rgba);
					glFunc->glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, CC_DEFAULT_CLOUD_SHININESS);
					glFunc->glPushAttrib(GL_LIGHTING_BIT);
					glFunc->glEnable(GL_LIGHTING);
				}

				glFunc->glBegin(GL_POINTS);
				executeFunctionForAllCellsAtLevel(m_displayedLevel,
				                                  &DrawCellAsAPoint,
				                                  additionalParameters);
				glFunc->glEnd();

				if (glParams.showNorms)
				{
					glFunc->glPopAttrib(); // GL_LIGHTING_BIT
				}
			}
			else if (m_displayMode == MEAN_CUBES)
			{
				// by default we use a box as primitive
				PointCoordinateType cs = getCellSize(m_displayedLevel);
				CCVector3           dims(cs, cs, cs);
				ccBox               box(dims);
				box.showColors(glParams.showColors || glParams.showSF);
				box.showNormals(glParams.showNorms);

				// trick: replace all normal indexes so that they point on the first one
				{
					if (box.arePerTriangleNormalsEnabled())
						for (unsigned i = 0; i < box.size(); ++i)
							box.setTriangleNormalIndexes(i, 0, 0, 0);
				}

				// fake context
				CC_DRAW_CONTEXT fakeContext = context;
				fakeContext.drawingFlags    = CC_DRAW_3D | CC_DRAW_FOREGROUND | CC_LIGHT_ENABLED;
				fakeContext.display         = nullptr;

				void* additionalParameters[] = {reinterpret_cast<void*>(&glParams),
				                                reinterpret_cast<void*>(m_theAssociatedCloudAsGPC),
				                                reinterpret_cast<void*>(&box),
				                                reinterpret_cast<void*>(&fakeContext)};

				executeFunctionForAllCellsAtLevel(m_displayedLevel,
				                                  &DrawCellAsAPrimitive,
				                                  additionalParameters);
			}
			else
			{
				assert(false);
			}

			glFunc->glEndList();
			m_glListIsDeprecated = false;
		}

		glFunc->glCallList(m_glListID);

		if (glParams.showNorms)
		{
			glFunc->glDisable(GL_COLOR_MATERIAL);
			// DGM FIXME: is it still true with Qt5.4+?
			glFunc->glDisable(GL_RESCALE_NORMAL);
			glFunc->glDisable(GL_LIGHTING);
		}
	}

	glFunc->glPopAttrib();
}

bool ccOctree::DrawCellAsABox(const CCCoreLib::DgmOctree::octreeCell& cell,
                              void**                                  additionalParameters,
                              CCCoreLib::NormalizedProgress*          nProgress /*=nullptr*/)
{
	ccOctreeFrustumIntersector* ofi    = static_cast<ccOctreeFrustumIntersector*>(additionalParameters[0]);
	QOpenGLFunctions_2_1*       glFunc = static_cast<QOpenGLFunctions_2_1*>(additionalParameters[1]);
	assert(glFunc != nullptr);

	CCVector3 bbMin;
	CCVector3 bbMax;
	cell.parentOctree->computeCellLimits(cell.truncatedCode, cell.level, bbMin, bbMax, true);

	ccOctreeFrustumIntersector::OctreeCellVisibility vis = ccOctreeFrustumIntersector::CELL_OUTSIDE_FRUSTUM;
	if (ofi)
		vis = ofi->positionFromFrustum(cell.truncatedCode, cell.level);

	// outside
	if (vis == ccOctreeFrustumIntersector::CELL_OUTSIDE_FRUSTUM)
	{
		ccGL::Color(glFunc, ccColor::green);
	}
	else
	{
		glFunc->glPushAttrib(GL_LINE_BIT);
		glFunc->glLineWidth(2.0f);
		// inside
		if (vis == ccOctreeFrustumIntersector::CELL_INSIDE_FRUSTUM)
			ccGL::Color(glFunc, ccColor::magenta);
		// intersecting
		else
			ccGL::Color(glFunc, ccColor::blue);
	}

	glFunc->glBegin(GL_LINE_LOOP);
	ccGL::Vertex3v(glFunc, bbMin.u);
	ccGL::Vertex3(glFunc, bbMax.x, bbMin.y, bbMin.z);
	ccGL::Vertex3(glFunc, bbMax.x, bbMax.y, bbMin.z);
	ccGL::Vertex3(glFunc, bbMin.x, bbMax.y, bbMin.z);
	glFunc->glEnd();

	glFunc->glBegin(GL_LINE_LOOP);
	ccGL::Vertex3(glFunc, bbMin.x, bbMin.y, bbMax.z);
	ccGL::Vertex3(glFunc, bbMax.x, bbMin.y, bbMax.z);
	ccGL::Vertex3v(glFunc, bbMax.u);
	ccGL::Vertex3(glFunc, bbMin.x, bbMax.y, bbMax.z);
	glFunc->glEnd();

	glFunc->glBegin(GL_LINES);
	ccGL::Vertex3v(glFunc, bbMin.u);
	ccGL::Vertex3(glFunc, bbMin.x, bbMin.y, bbMax.z);
	ccGL::Vertex3(glFunc, bbMax.x, bbMin.y, bbMin.z);
	ccGL::Vertex3(glFunc, bbMax.x, bbMin.y, bbMax.z);
	ccGL::Vertex3(glFunc, bbMax.x, bbMax.y, bbMin.z);
	ccGL::Vertex3v(glFunc, bbMax.u);
	ccGL::Vertex3(glFunc, bbMin.x, bbMax.y, bbMin.z);
	ccGL::Vertex3(glFunc, bbMin.x, bbMax.y, bbMax.z);
	glFunc->glEnd();

	// not outside
	if (vis != ccOctreeFrustumIntersector::CELL_OUTSIDE_FRUSTUM)
	{
		glFunc->glPopAttrib();
	}

	return true;
}

bool ccOctree::DrawCellAsAPoint(const CCCoreLib::DgmOctree::octreeCell& cell,
                                void**                                  additionalParameters,
                                CCCoreLib::NormalizedProgress*          nProgress /*=nullptr*/)
{
	// variables additionnelles
	glDrawParams*         glParams = reinterpret_cast<glDrawParams*>(additionalParameters[0]);
	ccGenericPointCloud*  cloud    = reinterpret_cast<ccGenericPointCloud*>(additionalParameters[1]);
	QOpenGLFunctions_2_1* glFunc   = static_cast<QOpenGLFunctions_2_1*>(additionalParameters[2]);
	assert(glFunc != nullptr);

	if (glParams->showSF)
	{
		ScalarType          dist = CCCoreLib::ScalarFieldTools::computeMeanScalarValue(cell.points);
		const ccColor::Rgb* col  = cloud->geScalarValueColor(dist);
		ccGL::Color(glFunc, col ? *col : ccColor::lightGreyRGB);
	}
	else if (glParams->showColors)
	{
		ccColor::Rgb col = ComputeAverageColor(cell.points, cloud);
		ccGL::Color(glFunc, col);
	}

	if (glParams->showNorms)
	{
		CCVector3 N = ComputeAverageNorm(cell.points, cloud);
		ccGL::Normal3v(glFunc, N.u);
	}

	const CCVector3* gravityCenter = CCCoreLib::Neighbourhood(cell.points).getGravityCenter();
	ccGL::Vertex3v(glFunc, gravityCenter->u);

	return true;
}

bool ccOctree::DrawCellAsAPrimitive(const CCCoreLib::DgmOctree::octreeCell& cell,
                                    void**                                  additionalParameters,
                                    CCCoreLib::NormalizedProgress*          nProgress /*=nullptr*/)
{
	// variables additionnelles
	glDrawParams*        glParams  = reinterpret_cast<glDrawParams*>(additionalParameters[0]);
	ccGenericPointCloud* cloud     = reinterpret_cast<ccGenericPointCloud*>(additionalParameters[1]);
	ccGenericPrimitive*  primitive = reinterpret_cast<ccGenericPrimitive*>(additionalParameters[2]);
	CC_DRAW_CONTEXT*     context   = reinterpret_cast<CC_DRAW_CONTEXT*>(additionalParameters[3]);

	// get the set of OpenGL functions (version 2.1)
	QOpenGLFunctions_2_1* glFunc = context->glFunctions<QOpenGLFunctions_2_1>();
	assert(glFunc != nullptr);

	if (glFunc == nullptr)
		return false;

	CCVector3 cellCenter;
	cell.parentOctree->computeCellCenter(cell.truncatedCode, cell.level, cellCenter, true);

	if (glParams->showSF)
	{
		ScalarType          dist = CCCoreLib::ScalarFieldTools::computeMeanScalarValue(cell.points);
		const ccColor::Rgb* rgb  = cloud->geScalarValueColor(dist);
		if (rgb)
			primitive->setColor(*rgb);
	}
	else if (glParams->showColors)
	{
		ccColor::Rgb col = ComputeAverageColor(cell.points, cloud);
		primitive->setColor(col);
	}

	if (glParams->showNorms)
	{
		CCVector3 N = ComputeAverageNorm(cell.points, cloud);
		if (primitive->getTriNormsTable())
		{
			// only one normal!
			primitive->getTriNormsTable()->setValue(0, ccNormalVectors::GetNormIndex(N.u));
		}
	}

	glFunc->glPushMatrix();
	ccGL::Translate(glFunc, cellCenter.x, cellCenter.y, cellCenter.z);
	primitive->draw(*context);
	glFunc->glPopMatrix();

	return true;
}

ccColor::Rgb ccOctree::ComputeAverageColor(CCCoreLib::ReferenceCloud* subset, ccGenericPointCloud* sourceCloud)
{
	if (!subset || subset->size() == 0 || !sourceCloud)
		return {};

	assert(sourceCloud->hasColors());
	assert(subset->getAssociatedCloud() == static_cast<CCCoreLib::GenericIndexedCloud*>(sourceCloud));

	Tuple3Tpl<double> sum(0, 0, 0);

	unsigned n = subset->size();
	for (unsigned i = 0; i < n; ++i)
	{
		const ccColor::Rgb& _theColor = sourceCloud->getPointColor(subset->getPointGlobalIndex(i));
		sum.x += _theColor.r;
		sum.y += _theColor.g;
		sum.z += _theColor.b;
	}

	return ccColor::Rgb(static_cast<ColorCompType>(sum.x / n),
	                    static_cast<ColorCompType>(sum.y / n),
	                    static_cast<ColorCompType>(sum.z / n));
}

CCVector3 ccOctree::ComputeAverageNorm(CCCoreLib::ReferenceCloud* subset, ccGenericPointCloud* sourceCloud)
{
	CCVector3 N(0, 0, 0);

	if (!subset || subset->size() == 0 || !sourceCloud)
		return N;

	assert(sourceCloud->hasNormals());
	assert(subset->getAssociatedCloud() == static_cast<CCCoreLib::GenericIndexedCloud*>(sourceCloud));

	unsigned n = subset->size();
	for (unsigned i = 0; i < n; ++i)
	{
		const CCVector3& Ni = sourceCloud->getPointNormal(subset->getPointGlobalIndex(i));
		N += Ni;
	}

	N.normalize();
	return N;
}

bool ccOctree::intersectWithFrustum(ccCameraSensor* sensor, std::vector<unsigned>& inCameraFrustum)
{
	if (!sensor)
		return false;

	// initialization
	float     globalPlaneCoefficients[6][4];
	CCVector3 globalCorners[8];
	CCVector3 globalEdges[6];
	CCVector3 globalCenter;
	sensor->computeGlobalPlaneCoefficients(globalPlaneCoefficients, globalCorners, globalEdges, globalCenter);

	if (!m_frustumIntersector)
	{
		m_frustumIntersector = new ccOctreeFrustumIntersector();
		if (!m_frustumIntersector->build(this))
		{
			ccLog::Warning("[ccOctree::intersectWithFrustum] Not enough memory!");
			return false;
		}
	}

	// get points of cells in frustum
	std::vector<std::pair<unsigned, CCVector3>> pointsToTest;
	m_frustumIntersector->computeFrustumIntersectionWithOctree(pointsToTest, inCameraFrustum, globalPlaneCoefficients, globalCorners, globalEdges, globalCenter);

	// project points
	for (size_t i = 0; i < pointsToTest.size(); i++)
	{
		if (sensor->isGlobalCoordInFrustum(pointsToTest[i].second /*, false*/))
			inCameraFrustum.push_back(pointsToTest[i].first);
	}

	return true;
}

bool ccOctree::pointPicking(const CCVector2d&           clickPos,
                            const ccGLCameraParameters& camera,
                            PointDescriptor&            output,
                            double                      pickWidth_pix /*=3.0*/) const
{
	output.point       = nullptr;
	output.squareDistd = -1.0;

	if (!m_theAssociatedCloudAsGPC)
	{
		assert(false);
		return false;
	}

	if (m_thePointsAndTheirCellCodes.empty())
	{
		// nothing to do
		return false;
	}

	CCVector3d clickPosd(clickPos.x, clickPos.y, 0.0);
	CCVector3d X(0, 0, 0);
	if (!camera.unproject(clickPosd, X))
	{
		return false;
	}

	ccGLMatrix trans;
	bool       hasGLTrans = m_theAssociatedCloudAsGPC->getAbsoluteGLTransformation(trans);

	// compute 3D picking 'ray'
	CCVector3 rayAxis;
	CCVector3 rayOrigin;
	{
		CCVector3d clickPosd2(clickPos.x, clickPos.y, 1.0);
		CCVector3d Y(0, 0, 0);
		if (!camera.unproject(clickPosd2, Y))
		{
			return false;
		}

		rayAxis   = (Y - X).toPC();
		rayOrigin = X.toPC();

		if (hasGLTrans)
		{
			ccGLMatrix iTrans = trans.inverse();
			iTrans.applyRotation(rayAxis);
			iTrans.apply(rayOrigin);
		}

		rayAxis.normalize(); // normalize afterwards as the local transformation may have a scale != 1
	}

	CCVector3 margin(0, 0, 0);
	double    maxFOV_rad = 0;
	if (camera.perspective)
	{
		maxFOV_rad = 0.002 * pickWidth_pix; // empirical conversion from pixels to FOV angle (in radians)
	}
	else
	{
		double maxRadius = pickWidth_pix * camera.fov_deg / 2;
		margin           = CCVector3(1, 1, 1) * static_cast<PointCoordinateType>(maxRadius);
	}

	// first test with the total bounding box
	CCCoreLib::Ray<PointCoordinateType> ray(rayAxis, rayOrigin);
	if (!CCCoreLib::AABB<PointCoordinateType>(m_dimMin - margin, m_dimMax + margin).intersects(ray))
	{
		// no intersection
		return true; // DGM: false would mean that an error occurred! (output.point == 0 means that nothing has been found)
	}

	// no need to go too deep
	const unsigned char maxLevel = findBestLevelForAGivenPopulationPerCell(10);

	// starting level of subdivision
	unsigned char level = 1;
	// binary shift for cell code truncation at current level
	unsigned char currentBitDec = GET_BIT_SHIFT(level);
	// current cell code
	CellCode currentCellCode          = INVALID_CELL_CODE;
	CellCode currentCellTruncatedCode = INVALID_CELL_CODE;
	// whether the current cell should be skipped or not
	bool skipThisCell = false;

#ifdef DEBUG_PICKING_MECHANISM
	m_theAssociatedCloud->enableScalarField();
#endif

	// ray with origin expressed in the local coordinate system!
	CCCoreLib::Ray<PointCoordinateType> rayLocal(rayAxis, rayOrigin - m_dimMin);

	// visibility table (if any)
	const ccGenericPointCloud::VisibilityTableType* visTable = m_theAssociatedCloudAsGPC->isVisibilityTableInstantiated() ? &m_theAssociatedCloudAsGPC->getTheVisibilityArray() : nullptr;

	// scalar field with hidden values (if any)
	ccScalarField* activeSF = nullptr;
	if (m_theAssociatedCloudAsGPC->sfShown()
	    && m_theAssociatedCloudAsGPC->isA(CC_TYPES::POINT_CLOUD)
	    && !visTable // if the visibility table is instantiated, we always display ALL points
	)
	{
		ccPointCloud*  pc = static_cast<ccPointCloud*>(m_theAssociatedCloudAsGPC);
		ccScalarField* sf = pc->getCurrentDisplayedScalarField();
		if (sf && sf->mayHaveHiddenValues() && sf->getColorScale())
		{
			// we must take this SF display parameters into account as some points may be hidden!
			activeSF = sf;
		}
	}

	// let's sweep through the octree
	for (cellsContainer::const_iterator it = m_thePointsAndTheirCellCodes.begin(); it != m_thePointsAndTheirCellCodes.end(); ++it)
	{
		CellCode truncatedCode = (it->theCode >> currentBitDec);

		// new cell?
		if (truncatedCode != currentCellTruncatedCode)
		{
			// look for the biggest 'parent' cell that englobes this cell and the previous one (if any)
			while (level > 1)
			{
				unsigned char bitDec = GET_BIT_SHIFT(level - 1);
				if ((it->theCode >> bitDec) == (currentCellCode >> bitDec))
				{
					// same parent cell, we can stop here
					break;
				}
				--level;
			}

			currentCellCode = it->theCode;

			// now try to go deeper with the new cell
			while (level < maxLevel)
			{
				Tuple3i cellPos;
				getCellPos(it->theCode, level, cellPos, false);

				// first test with the total bounding box
				PointCoordinateType halfCellSize = getCellSize(level) / 2;
				CCVector3           cellCenter((2 * cellPos.x + 1) * halfCellSize,
                                     (2 * cellPos.y + 1) * halfCellSize,
                                     (2 * cellPos.z + 1) * halfCellSize);

				CCVector3 halfCell = CCVector3(halfCellSize, halfCellSize, halfCellSize);

				if (camera.perspective)
				{
					double radialSqDist   = 0.0;
					double sqDistToOrigin = 0.0;
					rayLocal.squareDistances(cellCenter, radialSqDist, sqDistToOrigin);

					double dx      = sqrt(sqDistToOrigin);
					double dy      = std::max<double>(0, sqrt(radialSqDist) - CCCoreLib::SQRT_3 * halfCellSize);
					double fov_rad = atan2(dy, dx);

					skipThisCell = (fov_rad > maxFOV_rad);
				}
				else
				{
					skipThisCell = !CCCoreLib::AABB<PointCoordinateType>(cellCenter - halfCell - margin,
					                                                     cellCenter + halfCell + margin)
					                    .intersects(rayLocal);
				}

				if (skipThisCell)
					break;
				else
					++level;
			}

			currentBitDec            = GET_BIT_SHIFT(level);
			currentCellTruncatedCode = (currentCellCode >> currentBitDec);
		}

#ifdef DEBUG_PICKING_MECHANISM
		m_theAssociatedCloud->setPointScalarValue(it->theIndex, level);
#endif

		if (!skipThisCell)
		{
			// we shouldn't test points that are actually hidden!
			if ((!visTable || visTable->at(it->theIndex) == CCCoreLib::POINT_VISIBLE)
			    && (!activeSF || activeSF->getColor(activeSF->getValue(it->theIndex))))
			{
				// test the point
				const CCVector3* P = m_theAssociatedCloud->getPoint(it->theIndex);
				CCVector3        Q = *P;
				if (hasGLTrans)
				{
					trans.apply(Q);
				}

				CCVector3d Q2D;
				bool       insideFrustum = false;
				camera.project(Q, Q2D, &insideFrustum);
				if (insideFrustum)
				{
					if (std::abs(Q2D.x - clickPos.x) <= pickWidth_pix
					    && std::abs(Q2D.y - clickPos.y) <= pickWidth_pix)
					{
						double squareDist = CCVector3d(X.x - Q.x, X.y - Q.y, X.z - Q.z).norm2d();
						if (!output.point || squareDist < output.squareDistd)
						{
							output.point       = P;
							output.pointIndex  = it->theIndex;
							output.squareDistd = squareDist;
						}
					}
				}
			}
		}
	}

	return true;
}

PointCoordinateType ccOctree::GuessNaiveRadius(ccGenericPointCloud* cloud)
{
	if (!cloud)
	{
		assert(false);
		return 0;
	}

	PointCoordinateType largestDim = cloud->getOwnBB().getMaxBoxDim();

	return largestDim / std::min<unsigned>(100, std::max<unsigned>(1, cloud->size() / 100));
}

PointCoordinateType ccOctree::GuessBestRadiusAutoComputeOctree(ccGenericPointCloud*    cloud,
                                                               const BestRadiusParams& params,
                                                               QWidget*                parentWidget /*=nullptr*/)
{
	if (!cloud)
	{
		assert(false);
		return 0;
	}

	if (!cloud->getOctree())
	{
		ccProgressDialog pDlg(true, parentWidget);
		if (!cloud->computeOctree(&pDlg))
		{
			ccLog::Error(tr("Could not compute octree for cloud '%1'").arg(cloud->getName()));
			return 0;
		}
	}

	return ccOctree::GuessBestRadius(cloud, params, cloud->getOctree().data());
}

PointCoordinateType ccOctree::GuessBestRadius(ccGenericPointCloud*                cloud,
                                              const BestRadiusParams&             params,
                                              CCCoreLib::DgmOctree*               inputOctree /*=nullptr*/,
                                              CCCoreLib::GenericProgressCallback* progressCb /*=nullptr*/)
{
	if (!cloud)
	{
		assert(false);
		return 0;
	}

	CCCoreLib::DgmOctree* octree = inputOctree;
	if (!octree)
	{
		octree = new CCCoreLib::DgmOctree(cloud);
		if (octree->build(progressCb) <= 0)
		{
			delete octree;
			ccLog::Warning("[GuessBestRadius] Failed to compute the cloud octree");
			return 0;
		}
	}

	PointCoordinateType bestRadius = GuessNaiveRadius(cloud);
	if (bestRadius == 0)
	{
		ccLog::Warning("[GuessBestRadius] The cloud has invalid dimensions");
		return 0;
	}

	if (cloud->size() < 100)
	{
		// no need to do anything else for very small clouds!
		return bestRadius;
	}

	// we are now going to sample the cloud so as to compute statistics on the density
	{
		const unsigned sampleCount = std::min<unsigned>(200, cloud->size() / 10);

		double              aimedPop    = params.aimedPopulationPerCell;
		PointCoordinateType radius      = bestRadius;
		PointCoordinateType lastRadius  = radius;
		double              lastMeanPop = 0;

		std::random_device                      rd;        // non-deterministic generator
		std::mt19937                            gen(rd()); // to seed mersenne twister.
		std::uniform_int_distribution<unsigned> dist(0, cloud->size() - 1);

		// we may have to do this several times
		for (size_t attempt = 0; attempt < 10; ++attempt)
		{
			int totalCount       = 0;
			int totalSquareCount = 0;
			int minPop           = 0;
			int maxPop           = 0;
			int aboveMinPopCount = 0;

			unsigned char octreeLevel = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(radius);

			for (size_t i = 0; i < sampleCount; ++i)
			{
				unsigned randomIndex = dist(gen);
				assert(randomIndex < cloud->size());

				const CCVector3*                    P = cloud->getPoint(randomIndex);
				CCCoreLib::DgmOctree::NeighboursSet Yk;
				int                                 n = octree->getPointsInSphericalNeighbourhood(*P, radius, Yk, octreeLevel);
				assert(n >= 1);

				totalCount += n;
				totalSquareCount += n * n;
				if (i == 0)
				{
					minPop = maxPop = n;
				}
				else
				{
					if (n < minPop)
						minPop = n;
					else if (n > maxPop)
						maxPop = n;
				}

				if (n >= params.minCellPopulation)
				{
					++aboveMinPopCount;
				}
			}

			double meanPop          = static_cast<double>(totalCount) / sampleCount;
			double stdDevPop        = sqrt(std::abs(static_cast<double>(totalSquareCount) / sampleCount - meanPop * meanPop));
			double aboveMinPopRatio = static_cast<double>(aboveMinPopCount) / sampleCount;

			ccLog::Print(QString("[GuessBestRadius] Radius = %1 -> samples population in [%2 ; %3] (mean %4 / std. dev. %5 / %6% above minimum)")
			                 .arg(radius)
			                 .arg(minPop)
			                 .arg(maxPop)
			                 .arg(meanPop)
			                 .arg(stdDevPop)
			                 .arg(aboveMinPopRatio * 100));

			if (std::abs(meanPop - aimedPop) < params.aimedPopulationRange)
			{
				// we have found a correct radius
				bestRadius = radius;

				if (aboveMinPopRatio < params.minAboveMinRatio)
				{
					// ccLog::Warning("[GuessBestRadius] The cloud density is very inhomogeneous! You may have to increase the radius to get valid normals everywhere... but the result will be smoother");
					aimedPop = params.aimedPopulationPerCell + (2.0 * stdDevPop) /* * (1.0-aboveMinPopRatio)*/;
					assert(aimedPop >= params.aimedPopulationPerCell);
				}
				else
				{
					break;
				}
			}

			// otherwise we have to find a better estimate for the radius
			double newRadius = radius;
			//(warning: we consider below that the number of points is proportional to the SURFACE of the neighborhood)
			assert(meanPop >= 1.0);
			if (attempt == 0)
			{
				// this is our best (only) guess for the moment
				bestRadius = radius;

				newRadius = radius * sqrt(aimedPop / meanPop);
			}
			else
			{
				// keep track of our best guess nevertheless
				if (std::abs(meanPop - aimedPop) < std::abs(bestRadius - aimedPop))
				{
					bestRadius = radius;
				}

				double slope           = (radius * radius - lastRadius * lastRadius) / (meanPop - lastMeanPop);
				double newSquareRadius = static_cast<double>(lastRadius) * lastRadius + (aimedPop - lastMeanPop) * slope;
				if (newSquareRadius > 0)
				{
					newRadius = sqrt(newSquareRadius);
				}
				else
				{
					// can't do any better!
					break;
				}
			}

			lastRadius  = radius;
			lastMeanPop = meanPop;

			radius = static_cast<PointCoordinateType>(newRadius);
		}
	}

	if (nullptr == inputOctree)
	{
		delete octree;
		octree = nullptr;
	}

	return bestRadius;
}
